
#include "base/utils/utils.h"
#include "base/types/SO3.h"
#include "network/Socket++.h"
#include "hardware/Gps/utils_GPS.h"
#include "base/utils/utils_math.h"

#include "utils_mavlink.h"
#include "VirtualUAV_Quad.h"


using namespace std;
using namespace pi;


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

#define PI          (3.1415926)
#define D2R         (3.1415926 / 180.0)
#define R2D         (180.0/3.1415926)
#define M2FT        (0.3048)                //transfer (m/sec^2) to (ft/sec^2)
#define R_EARTH     (6378100.0)


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

VirtualUAV_Quad::VirtualUAV_Quad()
{
    init();
}

VirtualUAV_Quad::~VirtualUAV_Quad()
{
    release();
}

int VirtualUAV_Quad::init()
{
    string uavName = fmt::sprintf("vuav_%d", ID);

    uavType         = MAV_TYPE_QUADROTOR;

    sim_m           = svar.GetDouble(uavName + ".m", 10.0);
    sim_dragK       = svar.GetDouble(uavName + ".dragK", 0.25);

    sim_Ax          = 0;
    sim_Ay          = 0;
    sim_Az          = 0;
    sim_Vx          = 0;
    sim_Vy          = 0;
    sim_Vz          = 0;

    sim_tLast       = 0;
    sim_tNow        = 0;
    sim_totalThrust = 0;

    m_simStep       = 0;

    svar.GetInt("UAS.UAV.uavStatus", 0) = 0;

    m_numMotor      = 4;
    for(int i=0; i<m_numMotor; i++) {
        svar.GetInt(fmt::sprintf("UAS.UAV.eng_state_%d", i), 2) = 0;
        svar.GetDouble(fmt::sprintf("UAS.UAV.rpm_%d", i), 2000) = 0;
    }

    return 0;
}

int VirtualUAV_Quad::release()
{
    return 0;
}

int VirtualUAV_Quad::timerFunction(void *arg)
{
    return VirtualUAV::timerFunction(arg);
}

//
// joystick defines
//
//  jsv->AXIS[0];         // roll
//  jsv->AXIS[1];         // pitch
//  jsv->AXIS[2];         // thrust
//  jsv->AXIS[3];         // yaw
//
int VirtualUAV_Quad::simulation(pi::JS_Val *jsv)
{
    const int   jsN = JS_AXIS_MAX_NUM;
    float       js[jsN];
    double      js_deadzone = svar.GetDouble("VirtualUAV.jsDeadZone", 0.015);

    double      attNoise = svar.GetDouble("VirtualUAV.AttitudeNoise", 0.01);
    double      attMaxAngle = svar.GetDouble("VirtualUAV.AttitudeMaxAngle", 30.0);

    double      lat1, lng1;
    double      dt;
    int         i;
    int         useSettedSpd = 0;


    // time
    sim_tNow = tm_getTimeStamp();
    dt = sim_tNow - sim_tLast;
    if( dt > 100 ) {
        sim_tLast = sim_tNow;
        return -1;
    }
    sim_tLast = sim_tNow;

    updateTime(sim_tNow);

    //
    svar.GetDouble("UAS.UAV.num_controlInput", 16) = jsN;

    // process input axis values
    for(i=0; i<jsN; i++) {
        js[i] = jsv->AXIS[i];

        svar.GetDouble(fmt::sprintf("UAS.UAV.controlInput_%d", i), 0) = js[i];

        if( fabs(js[i]) < js_deadzone ) js[i] = 0.0;
    }

    if( isArmed() ) {
        // detect is landed?
        if( (svar.GetInt("UAS.UAV.uavStatus", 0) == 1 || svar.GetInt("UAS.UAV.uavStatus", 0) == 3 ) &&
            gpH < svar.GetDouble("VirtualUAV.landHeightThreshold", 0.4) ) {
            gpH = 0.0;

            sim_Vx = 0.0;
            sim_Vy = 0.0;
            sim_Vz = 0.0;

            setArmed(0);
            setFlightMode(MFM_STABILIZE);

            useSettedSpd = 1;

            for(i=0; i<jsN; i++) js[i] = 0.0;

            svar.GetInt("UAS.UAV.uavStatus", 0) = 0;

            for(i=0; i<m_numMotor; i++) svar.GetInt(fmt::sprintf("UAS.UAV.eng_state_%d", i), 2) = 0;
        }

        // detect automatic landing
        if( fabs(jsv->BUTTON[svar.GetInt("VirtualUAV.autoLandingBtn", 11)]) > 0.001 ||
            svar.GetInt("UAS.UAV.uavStatus", 0) == 3 ) {
            setFlightMode(MFM_LAND);            // set flight mode to 'Land'

            sim_Vx = 0.0;
            sim_Vy = 0.0;
            sim_Vz = - svar.GetDouble("VirtualUAV.autoLandingSpeed", 5.0) + 0.01*(rand()%100 - 50);
            useSettedSpd = 1;

            for(i=0; i<jsN; i++) js[i] = 0.0;

            svar.GetInt("UAS.UAV.uavStatus", 0) = 3;
        }

        // during takeoff
        if( svar.GetInt("UAS.UAV.uavStatus", 0) == 2 ) {
            if( gpH >= svar.GetDouble("VirtualUAV.takeoffHeightThreshold", 15) ) {
                svar.GetInt("UAS.UAV.uavStatus", 0) = 1;
            }
        }

        // auto mode
        if( customMode == MFM_AUTO ) {
            autoNavigate(js);
        } else {
            svar.GetDouble("UAS.UAV.driftAngle", 0) = 0.0;
        }
    } else {
        // detect arm button
        if( fabs(jsv->BUTTON[svar.GetInt("VirtualUAV.armBtn", 10)]) > 0.001 ) {
            customMode = 0; // stablize mode

            setArmed(1);
            svar.GetInt("UAS.UAV.uavStatus", 0) = 2;

            gpH = 0.0;
            sim_Vx = 0.0;
            sim_Vy = 0.0;
            sim_Vz = 0.0;
            useSettedSpd = 1;

            for(i=0; i<m_numMotor; i++) svar.GetInt(fmt::sprintf("UAS.UAV.eng_state_%d", i), 2) = 2;
        } else {
            gpH = 0.0;

            sim_Vx = 0.0;
            sim_Vy = 0.0;
            sim_Vz = 0.0;
            useSettedSpd = 1;

            for(i=0; i<jsN; i++) js[i] = 0.0;
        }
    }

    // detect flight too high warning
    if( gpH > svar.GetDouble("VirtualUAV.warnFlightHeight", 3000) ) {
        //useSettedSpd = 1;
        if( js[2] > 0 ) js[2] = 0;
    }

    // detect landgear button
    if( fabs(jsv->BUTTON[svar.GetInt("VirtualUAV.landgearBtn", 2)]) > 0.001 ) {
        if( sim_tNow - m_landgearTM > svar.GetDouble("VirtualUAV.landgearTimeInterval", 1) ) {
            if( m_landgearState ) m_landgearState = 0;
            else                  m_landgearState = 1;

            m_landgearTM = sim_tNow;

            svar.GetInt("UAS.UAV.landgearState", 0) = m_landgearState;
        }
    }


    // calculate roll, pitch, yaw
    double _yaw, _roll, _pitch;

    _roll  = attMaxAngle * js[0] * D2R;
    _pitch = attMaxAngle * js[1] * D2R;
    _yaw   = yaw * D2R;
    _yaw  += attMaxAngle * js[3] * D2R*dt;

    // add random noise to attitude
    if( svar.GetInt("UAS.UAV.uavStatus", 0) != 0 ) {
        _roll  += attNoise*D2R * 0.01*(rand()%200 - 100);
        _pitch += attNoise*D2R * 0.01*(rand()%200 - 100);
        _yaw   += attNoise*D2R * 0.01*(rand()%200 - 100);
    }

    // rotation matrix of Euler
    //    FIXME: angle need to be processed
    SO3d convert;
    convert.FromEuler(-_pitch, -_yaw + M_PI/2, _roll);

    // decrease the speed when joystick in netural position
    Point3d V_earth(sim_Vx, sim_Vy, sim_Vz);
    if( fabs(js[0]) < js_deadzone && fabs(js[1]) < js_deadzone ) {
        if( !useSettedSpd ) V_earth = V_earth*0.2;
    }

    // calculate drage force
    Point3d D_earth = -V_earth * V_earth.norm()*sim_dragK;

    // calc F_earth
    //   Point3d F_body(0.0,0.0,double(G*(1-js2)));
    Point3d F_body(0,0,1);
    Point3d F_earth =  convert * F_body;

    double G = sim_m * 9.8;
    double fVertical = G*(1+js[2]);
    if( fVertical < G/10.0 ) fVertical = G/10.0;

    double fratio = 1.0 / F_earth.z;
    F_earth = F_earth*(fratio*fVertical);

    // calc TotalThrust
    sim_totalThrust = fVertical;

    // calc  A_earth
    Point3d G_earth = Point3d(0, 0, -G);
    Point3d A_earth = (F_earth + G_earth + D_earth)/sim_m;

    // calc  V_earth
    if( !useSettedSpd ) {
        V_earth  = V_earth + A_earth*dt;

        V_earth.z = svar.GetDouble("VirtualUAV.navVSpeed", 10) * js[2];
    }

    // process horiztonal speed limitation
    double navHSped = svar.GetDouble("VirtualUAV.navHSpeed", 20);
    double hv = sqrt(V_earth.x*V_earth.x + V_earth.y*V_earth.y);
    if( hv > navHSped ) {
        V_earth.x = V_earth.x / hv * navHSped;
        V_earth.y = V_earth.y / hv * navHSped;
    }

    sim_Vx = V_earth.x;
    sim_Vy = V_earth.y;
    sim_Vz = V_earth.z;

    // calculate x,y,z offset in earth frame
    Point3d P_earth(0, 0, gpH);
    P_earth = P_earth + V_earth*dt;
    // FIXME: need DEM data to prevent below ground
    if( P_earth.z < 0 ) P_earth.z = 0;

    // convert dx,dy to lng, lat
    calcLngLatFromDistance(
                gpLon, gpLat,
                P_earth.x, P_earth.y,
                lng1, lat1);

    // update VirtualUAV information
    yawSpd          = (yaw * D2R - _yaw) / dt;
    rollSpd         = (roll * D2R - _roll) / dt;
    pitchSpd        = (pitch * D2R - _pitch ) / dt;
    yaw             = angleNormalize(_yaw * R2D);
    roll            = _roll  * R2D;
    pitch           = _pitch * R2D;

    gpLon           = lng1;
    gpLat           = lat1;
    gpH             = P_earth.z;
    gpAlt           = homeAlt + gpH;
    gpHeading       = yaw;

    lat             = gpLat;
    lon             = gpLon;
    alt             = gpAlt;
    gpsGroundSpeed  = sqrt(sim_Vx*sim_Vx + sim_Vy*sim_Vy);
    gpsFixType      = 3;

    if( m_simStep % 100 == 0 ) {
        cpuLoad         = 50.0 + (rand() % 20 - 10);

        nSat            = 20;
        HDOP_h          = 1 + 0.004*(rand()%100 - 50);
        HDOP_v          = 1.5 + 0.01*(rand()%100 - 50);

        double rssi = 100 + (1.0-1.0/(1+exp(-P_earth.norm()/1000))) * 200 + 0.1*(rand()%100-50);
        svar.GetDouble("UAS.Telem.RSSI", 100) = rssi;
        rssi = 100 + (1.0-1.0/(1+exp(-P_earth.norm()/1000))) * 200 + 0.1*(rand()%100-50);
        svar.GetDouble("UAS.Video.RSSI", 100) = rssi;

        battVolt        = svar.GetDouble("UAS.UAV.battVolt", 24.5) + 0.01*(rand() % 100 - 50);
        battCurrent     = V_earth.norm() + 0.01*(rand() % 100 - 50);
        if( battCurrent < 0 ) battCurrent = 0;
        battRemaining   = 80.0;
        commDropRate    = 0.0;
    }

    float rpm = (sim_totalThrust/sim_m)*100;
    svar.GetInt("UAS.UAV.num_engines", 4) = m_numMotor;
    for(i=0; i<m_numMotor; i++) {
        svar.GetDouble(fmt::sprintf("UAS.UAV.rpm_%d", i), 2000) = rpm;
    }


    // update Flightgear data
    m_flightGearData.altitude  = gpAlt;
    m_flightGearData.longitude = gpLon;
    m_flightGearData.latitude  = gpLat;

    m_flightGearData.A_X_pilot = A_earth.x;
    m_flightGearData.A_Y_pilot = A_earth.y;
    m_flightGearData.A_Z_pilot = A_earth.z;

    m_flightGearData.v_body_u  = V_earth.x;
    m_flightGearData.v_body_v  = V_earth.y;
    m_flightGearData.v_body_w  = V_earth.z;

    // print information
    if( svar.GetInt("VirtualUAV.ShowSimulationInfo", 0) ) {
        printf("js               = %f %f %f %f\n", js[0], js[1], js[2], js[3]);
        printf("dt               = %f\n", dt);
        printf("roll, pitch, yaw = %f %f %f\n", roll, pitch, yaw);

        cout<<"SO3              = " << convert << endl;
        cout<<"D_earth          = " << D_earth << endl;
        cout<<"F_earth          = " << F_earth << "(" << fratio << ")\n";
        cout<<"A_earth          = " << A_earth << endl;
        cout<<"V_earth          = " << V_earth << endl;
        cout<<"d(x,y,z)         = " << P_earth << endl;

        printf("totalThrust      = %f\n", sim_totalThrust);
        printf("Position         = %f %f %f\n",
               m_flightGearData.latitude, m_flightGearData.longitude, m_flightGearData.altitude);
        printf("\n");
    }

    return 0;
}

int VirtualUAV_Quad::autoNavigate(float *js)
{
    double      navHSpd = svar.GetDouble("VirtualUAV.navHSpeed", 20);
    double      navVSpd = svar.GetDouble("VirtualUAV.navVSpeed", 10);

    // check mission array
    if( m_missionArray.size() <= 0 ) {
        setFlightMode(MFM_STABILIZE);           // change flight mode to 'stablize'
        return 0;
    }

    // calculate distance to target
    double              dx, dy, gd, dz;
    MissionData_Simp    &md = m_missionArray[m_missionIndex];

    calcLngLatDistance(gpLon, gpLat, md.lng, md.lat,
                       dx, dy);
    dz = md.alt - gpH;                          // height difference
    gd = sqrt(dx*dx + dy*dy);                   // ground distance

    // goto next waypoint
    if( sqrt(dx*dx+dy*dy+dz*dz) < svar.GetDouble("VirtualUAV.autoNavigate.distThreshold", 5.0) ) {
        m_missionIndex ++;
        if( m_missionIndex >= m_missionNum ) {
            m_missionIndex = 0;
            setFlightMode(MFM_STABILIZE);       // change flight mode to 'stablize'
        }

        return 0;
    }

    // calculate reach time
    double t1 = gd / navHSpd;
    double t2 = dz / navVSpd;

    // calc throttle value
    double vz = navVSpd;
    if( t1 > t2 ) vz = dz / t1;
    js[2] = vz / navVSpd;               // throttle

    // calc roll/pitch
    double a_pitch = -yaw*D2R + M_PI/2.0;
    double a_roll  = -yaw*D2R;

    double d_pitch = dx*cos(a_pitch) + dy*sin(a_pitch);
    double d_roll  = dx*cos(a_roll)  + dy*sin(a_roll);
    double d_sum   = sqrt(d_pitch*d_pitch + d_roll*d_roll);
    js[0] =  d_roll  / d_sum;           // roll
    js[1] = -d_pitch / d_sum;           // pitch

    // calc drift angle to airway
    double a_airway = atan2(dy, dx);

    double driftAngle = -(a_airway - a_pitch);
    if( driftAngle >  M_PI ) driftAngle = M_PI*2 - driftAngle;
    if( driftAngle < -M_PI ) driftAngle = M_PI*2 + driftAngle;
    svar.GetDouble("UAS.UAV.driftAngle", 0) = driftAngle*R2D;

    return 0;
}


int VirtualUAV_Quad::toFlightGear(FGNetFDM *fgData)
{
    FGNetFDM &fdm = *fgData;

    fdm.phi                 = convByteOrder_h2n<float>(roll  * D2R );
    fdm.theta               = convByteOrder_h2n<float>(pitch * D2R );
    fdm.psi                 = convByteOrder_h2n<float>(yaw   * D2R );

    // X, Y, Z accel in body frame ft/sec^2
    fdm.A_X_pilot           = convByteOrder_h2n<float>(m_flightGearData.A_X_pilot * M2FT);
    fdm.A_Y_pilot           = convByteOrder_h2n<float>(m_flightGearData.A_Y_pilot * M2FT);
    fdm.A_Z_pilot           = convByteOrder_h2n<float>(m_flightGearData.A_Z_pilot * M2FT);

    fdm.v_body_u            = convByteOrder_h2n<float>(m_flightGearData.v_body_u);
    fdm.v_body_v            = convByteOrder_h2n<float>(m_flightGearData.v_body_v);
    fdm.v_body_w            = convByteOrder_h2n<float>(m_flightGearData.v_body_w);

    fdm.longitude           = convByteOrder_h2n<double>(m_flightGearData.longitude * D2R);
    fdm.latitude            = convByteOrder_h2n<double>(m_flightGearData.latitude  * D2R);
    fdm.altitude            = convByteOrder_h2n<double>(m_flightGearData.altitude);

    float rpm               = (sim_totalThrust/sim_m)*1000;
    fdm.num_engines         = convByteOrder_h2n<uint32_t>(4);
    fdm.rpm[0]              = convByteOrder_h2n<float>(rpm);
    fdm.rpm[1]              = convByteOrder_h2n<float>(rpm);
    fdm.rpm[2]              = convByteOrder_h2n<float>(rpm);
    fdm.rpm[3]              = convByteOrder_h2n<float>(rpm);

    fdm.num_tanks           = convByteOrder_h2n<uint32_t>(1);
    fdm.fuel_quantity[0]    = convByteOrder_h2n<float>(100.0);

    fdm.num_wheels          = convByteOrder_h2n<uint32_t>(3);

    fdm.cur_time            = convByteOrder_h2n<uint32_t>(tm_getTimeStampUnix());
    fdm.warp                = convByteOrder_h2n<uint32_t>(1);

    fdm.visibility          = convByteOrder_h2n<float>(m_flightGearData.visibility);

    fdm.version             = convByteOrder_h2n<uint32_t>(FG_NET_FDM_VERSION);

    return 0;
}
